#include "handdetector.h"

#include <iostream>
using namespace std;

using namespace cv;
using namespace std;
using namespace mlgesture;

const float HandDetector::FINGER_THRESHOLD = 0.5f;
const float HandDetector::VALLEY_THRESHOLD = 0.5f;
const float HandDetector::FALSE_THRESHOLD = -0.7f;

HandDetector::HandDetector(const Mat &src, int min_area) : minContourArea_(min_area) {
	imgSource_ = Mat::zeros(src.rows, src.cols, CV_8UC1);
	imgTemp_ = Mat::zeros(src.rows, src.cols, CV_8UC1);
}

HandDetector::~HandDetector() {
	clear();
}

void HandDetector::clearContours() {
	for (unsigned i = 0; i < contoursInfo_.size(); i++) {
		delete contoursInfo_[i];
	}
	contoursInfo_.clear();
	contours_.clear();
}

void HandDetector::clear() {
	clearContours();
	handsRect_.clear();
	potentialHands_.clear();
}

void HandDetector::process(const cv::Mat &img) throw (HandGestureException) {
	if (img.channels() != 1)
		throw HandGestureException("Image to process can have only one channel");
	clearContours();

	imgTemp_ = img.clone();
	cv::findContours(imgTemp_, contours_, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

	vector<vector<Point> >::iterator it_contour = contours_.begin(), it_contour_end = contours_.end();

	for(int i=0; it_contour != it_contour_end; ++it_contour, ++i) {
		ContourInfo* contour_tmp = new ContourInfo(i);
		vector<Point> approx;
		approxPolyDP(Mat(*it_contour), approx, 5, true);
		contour_tmp->area_ = contourArea(Mat(approx));
		Rect contour_rect = boundingRect(Mat(*it_contour));
		if (contour_tmp->area_ < minContourArea_ || abs(contour_rect.area() - img.size().area()) < 4500) {
			delete contour_tmp;
		}
		else contoursInfo_.push_back(contour_tmp);
	}

	vector<vector<Point> > contours;
	findContours(contours, 10);
	Mat tmp = Mat::zeros(img.rows, img.cols, CV_8UC1);
	drawContours(tmp, contours, -1, Scalar(255,255,255), 2);

	handsRect_.clear();
	vector<Rect> rects_tmp;
	for(vector<vector<Point> >::iterator it = contours.begin(); it != contours.end(); ++it) {
		Rect rect;

		while (1) {
			bool res = checkContour(*it, rect);
			if (!res)
				break;
			rects_tmp.push_back(rect);
			for(vector<Rect>::iterator it = potentialHands_.begin(); it != potentialHands_.end(); ++it) {
				Rect rect_itersect = *it & rect;
				int area_intersect = rect_itersect.area();
				int area1 = rect.area();
				int area2 = it->area();
				if (area_intersect >= 0.6*area1 && area_intersect <= 1.4*area1 &&
						area_intersect >= 0.6*area2 && area_intersect <= 1.4*area2) {
					handsRect_.push_back(rect_itersect);
					break;
				}
			}
		}
	}
	potentialHands_ = rects_tmp;
}

void HandDetector::findContours(vector<vector<Point> > &contours, int contoursToFind) {
	if (contoursToFind > 0) {
		sort(contoursInfo_.begin(), contoursInfo_.end(), cmpContourInfo());
	}

	if (contoursToFind < 0 || contoursToFind > (int)contoursInfo_.size()) {
		contoursToFind = contoursInfo_.size();
	}

	for (int i=0;  i<contoursToFind; i++) {
		contours.push_back(contours_[contoursInfo_[i]->id_]);
	}
}

void HandDetector::calculateParams(const vector<Point> &contour, vector<double> &cosine, vector<int> &direction) {
	int contour_size = contour.size();
	cosine.resize(contour_size);
	direction.resize(contour_size);
	int max_cos_point = (contour_size/500)*10 + 30;
	for(int i=0; i<contour_size; i++) {
		Point p = contour[i];
		float max_cos = -2.;
		int max_dir = -1;
		for(int j=20; j<=max_cos_point; j++) {
			Point a,b,vec_a,vec_b;
			int index1 = (i-j);
			if (index1 < 0) index1 = contour_size + index1;
			a=contour[index1];
			b=contour[(i+j)%contour_size];

			vec_a = a-p;
			vec_b = b-p;
			float cos_local = vec_a.dot(vec_b)/(sqrt((float)(vec_a.x*vec_a.x+vec_a.y*vec_a.y)*(vec_b.x*vec_b.x+vec_b.y*vec_b.y)));
			if (cos_local>max_cos) {
				max_cos = cos_local;
				int det = p.x*a.y+a.x*b.y+b.x*p.y-p.x*b.y-p.y*a.x-a.y*b.x;
				if (det<0) max_dir = -1;
				else if (det>0) max_dir = 1;
				else max_dir = 0;
			}
		}
		cosine[i] = max_cos;
		direction[i] = max_dir;
	}
}

void HandDetector::checkParams(const vector<double> &cosine, const vector<int> &direction,
							   vector<int> &contour_state, vector<int> &max_pos) {
	int contour_size = cosine.size();
	int finger_count = 0;
	int valley_count = 0;
	bool finger_begin = false;
	bool valley_begin = false;
	bool false_begin = false;
	bool is_finger = false;
	bool is_valley = false;
	bool is_false_peak = false;
	float local_max = -2;

	for(int i=0; i<contour_size; i++) {
		double cos = cosine[i];
		int dir = direction[i];

		if (cos > FINGER_THRESHOLD && dir == 1) {
			if (i==0) finger_begin = true;
			if (!is_finger) {
				if (is_false_peak) {
					contour_state.pop_back();
					max_pos.pop_back();
				}
				is_finger = true;
				finger_count++;
				contour_state.push_back(1);
				max_pos.push_back(i);
			}
			if (local_max < cos) {
				local_max = cos;
				max_pos.back() = i;
			}
			is_valley = false;
			is_false_peak = false;
		}
		else if (cos > VALLEY_THRESHOLD && dir == -1) {
			if (i==0) valley_begin = true;
			if (!is_valley) {
				if (is_false_peak) {
					contour_state.pop_back();
					max_pos.pop_back();
				}
				is_valley = true;
				valley_count++;
				contour_state.push_back(-1);
				max_pos.push_back(i);
			}
			is_finger = false;
			is_false_peak = false;
			local_max = -2;
		}
		else {
			if (cos > FALSE_THRESHOLD) {
				if (!is_false_peak) {
					if (!is_finger && !is_valley) {
						contour_state.push_back(0);
						max_pos.push_back(i);
					}
					if (i==0) false_begin = true;
					is_false_peak = true;
				}
			}
			else {
				is_false_peak = false;
			}
			is_finger = false;
			is_valley = false;
			local_max = -2;
		}
	}

	if ((is_finger && finger_begin) || (is_valley && valley_begin)) contour_state.pop_back();
	if ((is_finger || is_valley) && false_begin) {
		contour_state[0] = contour_state.back();
		contour_state.pop_back();
	}
	if (is_false_peak && contour_state[0] != 0) {
		contour_state.pop_back();
	}
}

bool HandDetector::analyzeSequence(const vector<int> contour_state, const vector<int> max_pos, int contour_size,
								   int &first_point, int &last_point, int &hand_points_number) {
	bool is_hand = false;
	if (contour_state.size()>=9) {
		unsigned idx, idx2;
		for (unsigned i=0; i<contour_state.size(); i++) {
			int sum = 0;
			idx = i+7;
			if (idx >= contour_state.size()) idx %= contour_state.size();
			int last2 = contour_state[idx];
			idx = i+8;
			if (idx >= contour_state.size()) idx %= contour_state.size();
			int last = contour_state[idx];
			int j, first_pos=-1;
			for (j=0; j<9; j++) {
				idx = i+j;
				if (idx >= contour_state.size()) idx %= contour_state.size();
				if (contour_state[idx] == 0) break;
				if (contour_state[idx] == -1 && last == -1) break;
				if (contour_state[idx] == 1 && last == 1 && last2 == 1) break;
				if (contour_state[idx] == 1 && last == 1) {
					first_pos = idx;
				}
				sum += contour_state[idx];
				last2 = last;
				last = contour_state[idx];
			}
			if (j==9 && sum == 1) {
				idx = first_pos + 1;
				if (idx >= contour_state.size()) idx %= contour_state.size();
				int dist_tmp;
				if (max_pos[first_pos] > max_pos[idx]) {
					dist_tmp = contour_size - max_pos[first_pos] + max_pos[idx];
				}
				else {
					dist_tmp = max_pos[idx] - max_pos[first_pos];
				}

				first_point = max_pos[first_pos] - 2 * dist_tmp;
				if (first_point < 0) {
					first_point = contour_size - first_point;
				}

				idx = first_pos + 8;
				idx2 = first_pos + 7;
				if (idx >= contour_state.size()) idx %= contour_state.size();
				if (idx2 >= contour_state.size()) idx2 %= contour_state.size();
				if (max_pos[idx2] > max_pos[idx]) {
					dist_tmp = contour_size - max_pos[idx2] + max_pos[idx];
				}
				else {
					dist_tmp = max_pos[idx] - max_pos[idx2];
				}

				last_point = max_pos[idx] + 2 * dist_tmp;
				if (last_point >= contour_size - 1) {
					last_point = last_point - contour_size;
				}

				if (first_point > last_point) {
					hand_points_number = contour_size - first_point + last_point;
				}
				else {
					hand_points_number = last_point - first_point + 1;
				}

				if (hand_points_number < 200)
					continue;
				else is_hand = true;

				for (int i = 0; i<9; i++) {
					idx2 = first_pos + i;
					idx = first_pos + i + 1;
					if (idx >= contour_state.size()) idx %= contour_state.size();
					if (idx2 >= contour_state.size()) idx2 %= contour_state.size();
					if (max_pos[idx2] > max_pos[idx]) {
						dist_tmp = contour_size - max_pos[idx2] + max_pos[idx];
					}
					else {
						dist_tmp = max_pos[idx] - max_pos[idx2];
					}

					if (1.*dist_tmp/hand_points_number < 0.04) {
						is_hand = false;
					}
				}
				if (!is_hand) {
					continue;
				}
				break;
			}
		}
	}
	return is_hand;
}

bool HandDetector::checkContour(vector<Point> &contour, Rect &hand) {
	vector<double> cosine;
	vector<int> direction;
	vector<int> contour_state;
	vector<int> max_pos;

	calculateParams(contour, cosine, direction);
	checkParams(cosine, direction, contour_state, max_pos);

	int first_point, last_point;
	int hand_points_number;
	bool is_hand = analyzeSequence(contour_state, max_pos, contour.size(), first_point, last_point, hand_points_number);

	hand = Rect();
	if (!is_hand) return false;

	vector<Point> contour_tmp;
	vector<Point> contour_tmp2;
	for (int i = 0; i < (int)contour.size(); i++) {
		if (first_point < last_point){
			if (i < first_point || i> last_point) {
				contour_tmp2.push_back(contour[i]);
			}
		}
		else {
			if (i > last_point && i < first_point) {
				contour_tmp2.push_back(contour[i]);
			}
		}
	}
	for (int i = 0; i <= hand_points_number; i++) {
		contour_tmp.push_back(contour[(i+first_point)%contour.size()]);
	}
	if (contour_tmp.size() != 0)
		hand = boundingRect(Mat(contour_tmp));
	else
		return false;

	contour = contour_tmp2;

	return true;
}

vector<Rect> HandDetector::getHands() const {
	return handsRect_;
}

bool HandDetector::ContourInfo::operator< (const HandDetector::ContourInfo &contour) const {
	return area_ < contour.area_;
}

bool HandDetector::ContourInfo::operator> (const HandDetector::ContourInfo &contour) const {
	return area_ > contour.area_;
}
